// Include files.
#include "Internal/NullDeleter.hpp"
#include "Internal/FpkException.hpp"
#include "SystematicRobot.hpp"
#include "Operation.hpp"
#include "Subscriber.hpp"

using namespace std;
using namespace boost;

typedef std::set<shared_ptr<Subscriber> >::iterator Iterator;
typedef std::set<Operation<Unspecified>*>::iterator OperationIterator;

/**
 *  Constructor for Subscriber.
 */
Subscriber::Subscriber() :
	m_robot(NULL),
	m_parent(NULL),
	m_isSubscribed(false)
{
}

/**
 *  Destructor for Subscriber.
 */
Subscriber::~Subscriber()
{
}

/**
 *  Cancels all operations that the subscriber launched.
 */
void Subscriber::CancelAllOperations()
{
	std::set<Operation<Unspecified>* > operations(m_operations);
	
	for(OperationIterator it = operations.begin(); it != operations.end(); ++it)
		(*it)->Cancel();
}

/**
 *  This function is run every periodic loop.
 *  
 *  This function is called every periodic loop when subscribed to the robot.
 *  See SystematicRobot documentation for more on periodic loops and subscription.
 *  This function should be overridden to handle the periodic Subscriber tasks.
 */
void Subscriber::HandlePeriodic()
{
}


/**
 *  Get a pointer to the subscribed SystematicRobot or a NULL pointer if the Subscriber doesn't have a SystematicRobot to subscribe to.
 *  
 *  @return A pointer to the subscribed SystematicRobot or a NULL pointer if the Subscriber doesn't have a SystematicRobot to subscribe to.
 */
SystematicRobot *Subscriber::GetRobot()
{
	return m_robot;
}


/**
 *  Set the SystematicRobot to subscribe to.
 *  
 *  @param robot A pointer to the SystematicRobot to be subscribed to.
 */
void Subscriber::SetRobot(SystematicRobot *robot)
{
	m_robot = robot;
	
	for (Iterator it = m_children.begin(); it != m_children.end(); ++it)
		(*it)->SetRobot(m_robot);
}

bool Subscriber::GetIsSubscribed() const
{
	return m_isSubscribed;
}

/**
 *  Set if the Subscriber is running and listening to SystematicRobot's events (e.g. Periodic Loop).
 *  
 *  @param isSubscribed If the Subscriber should be running and listening to SystematicRobot's events (e.g. Periodic Loop).
 */
void Subscriber::SetIsSubscribed(bool isSubscribed)
{
	m_isSubscribed = isSubscribed;
}

/**
 *  Add a child Subscriber that will depend on this subscriber to receive SystematicRobot event calls.
 *  
 *  @param subscriber A shared pointer to the Subscriber to be added.
 */
void Subscriber::AddChild(boost::shared_ptr<Subscriber> subscriber)
{
	// End function if child already added.
	if (subscriber->m_parent == this)
		return;
	
	if (subscriber->m_parent != NULL)
		throw FpkException("A subscriber can only have one parent subscriber.  In other words, you can not add a child subscriber when it is already a child subscriber of another subscriber.");
	
	subscriber->m_parent = this;
	
	if (GetRobot() != NULL)
		subscriber->SetRobot(GetRobot());
	
	m_children.insert(subscriber);
}

/**
 *  Add a child Subscriber that will depend on this subscriber to receive SystematicRobot event calls.
 *  
 *  @param subscriber A reference to the Subscriber to be added.
 */
void Subscriber::AddChild(Subscriber &subscriber)
{
	AddChild(boost::shared_ptr<Subscriber>(&subscriber, NullDeleter()));
}

/**
 *  Remove a child subscriber.
 *  
 *  @param subscriber A shared pointer to the Subscriber to be removed.
 */
void Subscriber::RemoveChild(shared_ptr<Subscriber> subscriber)
{
	m_children.erase(subscriber);
	subscriber->m_parent = NULL;
}

/**
 *  Remove an operation from the subscriber.
 *  
 *  @param operation A pointer to the Operation to be removed.
 */
void Subscriber::RemoveOperation(Operation<Unspecified> *operation)
{
	m_operations.erase(operation);
	RemoveChild(operation->m_pointerSelfHack);
	operation->m_pointerSelfHack.reset();
}

/**
 *  Manage internal FPK periodic loop handling for the Subscriber.
 */
void Subscriber::RunPeriodic()
{
	if (m_isSubscribed)
	{
		HandlePeriodic();
		
		for (Iterator it = m_children.begin(); it != m_children.end(); ++it)
			(*it)->RunPeriodic();
	}
}

/**
 *  Aquire all required control lock.
 */
void Subscriber::AquireLocks()
{
	set<void*>::iterator it;
	
	for (it = m_controlLocksRequired.begin(); it != m_controlLocksRequired.end(); ++it)
	{
		// For each control lock required.
		
		GetRobot()->LockControl(*this, (*it));
	}
}

/**
 *  Release all required control lock.
 */
void Subscriber::ReleaseLocks()
{
	// Release control locks.
	set<void*>::iterator it;
			
	for (it = m_controlLocksRequired.begin(); it != m_controlLocksRequired.end(); ++it)
	{
		// For each control lock required.
		
		GetRobot()->UnlockControl(*this, (*it));
	}
}

/**
 *  Run internal code to correctly launch operation.
 *  
 *  @param operation A shared pointer to operation to be launched.
 */
void Subscriber::LaunchInternal(shared_ptr<Operation<Unspecified> > operation)
{
	operation->m_pointerSelfHack = operation;
	m_operations.insert(operation.get());
	AddChild(operation);
	operation->HandleLaunch();
}
